#!/usr/bin/python
#
# To plot the message, do
# rxplot  -m . -b 60 -p 10 -r 10  /Wii_balance_weight/data[0]:data[1]:data[2]:data[3] 
#
import cwiid
import sys
import roslib; roslib.load_manifest('wii_balance')
import rospy
from std_msgs.msg import Float32MultiArray
from time import sleep

current_measurement = [0]*4;
current_raw = [0]*4;
def main():
	global named_calibration 
	global wiimote
	global pub_weight,pub_raw

	#connect to device and enable callback interface / continuous report mode
	#wiimote = cwiid.Wiimote(bdaddr="00:24:44:E2:BB:10")
	wiimote = cwiid.Wiimote(bdaddr="00:24:44:E2:BB:10")
	wiimote.rpt_mode = cwiid.RPT_EXT 
	wiimote.enable(cwiid.FLAG_MESG_IFC);
	wiimote.enable(cwiid.FLAG_CONTINUOUS); # with CONTINUOUS (98hz) without (64hz)

	#get calibration information (from Wii's memory)
	wiimote.request_status()
	balance_calibration = wiimote.get_balance_cal()
	named_calibration = {   'right_top': balance_calibration[0],
			            	'right_bottom': balance_calibration[1],
							'left_top': balance_calibration[2],
							'left_bottom': balance_calibration[3],
						}

	#publish ros service
	pub_weight = rospy.Publisher('Wii_balance_weight', Float32MultiArray, tcp_nodelay=True)
	pub_raw = rospy.Publisher('Wii_balance_raw', Float32MultiArray, tcp_nodelay=True)
	rospy.init_node('Wii_balance_talker', anonymous=True)

	#setup callback, do this after loading calibration
	wiimote.mesg_callback = callback
	r = rospy.Rate(100) # 100hz
	while not rospy.is_shutdown():
		#read result from current_measurement
		#send ros message
		#pub1.publish(data=current_measurement) 
		#pub2.publish(data=current_raw)
		r.sleep()
	wiimote.close()

def callback(mesg_list, time):
	for mesg in mesg_list:
		if mesg[0] ==  cwiid.MESG_BALANCE:
			#do calibration 
			i=0
			for sensor in ('left_bottom', 'right_top', 'right_bottom', 'left_top'):
				reading = mesg[1][sensor]
				calibration = named_calibration[sensor]
				if reading < calibration[1]:
					current_measurement[i] = (1700 * (reading - calibration[0]) / (calibration[1] - calibration[0]))/100.0
				else:
					current_measurement[i] = (1700 * (reading - calibration[1]) / (calibration[2] - calibration[1]) + 1700)/100.0
				current_raw[i] = reading
				i+=1
			pub_weight.publish(data=current_measurement) 
			pub_raw.publish(data=current_raw)
			
		elif mesg[0] ==  cwiid.MESG_ERROR:
			print "Error message received"
			global wiimote
			wiimote.close()
			exit(-1)
		else:
			print 'Unknown Report'

	return 
main()
